/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/helpInc/distance_check.h
 * @Description  : 该文件定义了与距离检查相关的数据结构和类，包括路径运行数据、目标数据和距离检查类。 
 *                 主要功能包括更新和管理路径的实际和理论距离，提供距离超限的触发机制， 
 *                 以及对路径节点的有效性进行检查。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:43:30
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once 
#include "z_types.h"
#include "rclcpp/rclcpp.hpp"
#include <mutex>
#include <thread>
#include <vector>
#include <map>
#include "trajectory.h"
#include "path.h"
#include "tools.h"
#include <string>
#include "public_kernel.h"
#include "timestamp.h"
//////////////////////////////////////////////////////////////////////////////
//   The class of class "CDisRunData".
struct DisRunData {
    float abs_progress;   // 当前路径的绝对距离
    unsigned short from_node;     // 路径起点
    unsigned short to_node;       // 路径终点
    float cur_real_size;   // 当前路径里程计计算走过的长度(m)
    float cur_theory_size; // 当前路径理论计算走过的长度(m)

    DisRunData() { Clear(); }

    void Clear() {
        cur_real_size = 0.0f;
        cur_theory_size = 0.0f;
        abs_progress = 0.0f;
        from_node = 65535;
        to_node = 65535;
    }

    void Update(unsigned short from_node, unsigned short to_node, float abs_progress) {
        this->from_node = from_node;
        this->to_node = to_node;
        this->abs_progress = abs_progress;
    }

    // 计算理论和实际的累加长度
    void AccumulatedDis(float diff_ms) {
        IntegralDis(GetRealVel(), GetTheoryVel(), diff_ms);
    }

    private:
    // 计算理论和实际的积分长度
    void IntegralDis(float real_vel, float theory_vel, float diff_ms) {
        const float kMinValue = 0.04f; // 0.04m/s 速度 以内不参与里程计计算
        if (fabs(real_vel) < kMinValue) {
            DEBUG_OUT("real_vel can ignore; so set vel is zero;real_vel=" << real_vel);
            real_vel = 0.0f;
        }
        if (fabs(theory_vel) < kMinValue) {
            DEBUG_OUT("theory_vel can ignore; so set vel is zero;theory_vel=" << theory_vel);
            theory_vel = 0.0f;
        }
        cur_real_size += real_vel * diff_ms / 1000.0f;
        cur_theory_size += theory_vel * diff_ms / 1000.0f;
        DEBUG_OUT("accumulatedDis:real_vel=" << real_vel << ",cur_real_size=" << cur_real_size
                                                << ",theory_vel=" << theory_vel << ",cur_theory_size=" << cur_theory_size
                                                << ",diff_ms=" << diff_ms);
    }

    // 默认500ms以内都是有效
    // 仿真调试模式下，不关心里程计的数据时间戳
    float GetRealVel(float timeout = 500) {
        float vel = 0.0f, time_diff_ms = 0.0f;
        const agv_msgs::msg::OdometryLite& odom = GetOdomData();

        if (time_diff_ms < timeout) {
            float fx = odom.twist.linear.x;
            float fy = odom.twist.linear.y;
            vel = sqrt(Square(fx) + Square(fy));
        } else {
            DEBUG_ERROR_OUT("error:GetRealVel: odom data is time out:time_diff_ms" << time_diff_ms
                << ",timeout=" << timeout);
        }
        return vel;
    }

    // 获取理论的规划速度值
    float GetTheoryVel() {
        float vel = 0.0f;
        const geometry_msgs::msg::TwistStamped& control = GetControlVel();
        float fx = control.twist.linear.x;
        float fy = control.twist.linear.y;
        vel = sqrt(Square(fx) + Square(fy));
        return vel;
    }
};

struct DisTargetData {
    unsigned short from_node; // 路径起点
    unsigned short to_node;   // 路径终点
    float total_size; // 路径的总长度

    DisTargetData() { Clear(); }

    void Clear() {
        total_size = 999.0f;
        from_node = 65535;
        to_node = 65535;
        DEBUG_OUT("DisTargetData clear");
    }

    void Update(unsigned short from_node, unsigned short to_node, float total_range) {
        this->from_node = from_node;
        this->to_node = to_node;
        this->total_size = total_range;
        DEBUG_OUT("DisTargetData update:FromNode=" << this->from_node << ",ToNode=" << this->to_node << ",total_size=" << this->total_size);
    }
};


//  The class of class "DisCheck".
class DisCheck {
public:
    DisCheck();
    ~DisCheck() = default;

    // 初始化函数
    bool Init();

    // 报距离超限制
    bool Trigger(std::string& msg);

    // 使能检测
    void SetEnable(bool enable);

    // 添加检测新的二维码ID
    bool AddNewPath(Path* path, Trajectory* traj);

    // 更新实时数据
    void UpdateProgRunData(unsigned short from_node, unsigned short to_node, float abs_progress);

    // 清除检测缓冲
    void ClearCheckBuffer();

private:
    // 复位清除当前状态
    void Reset();

    // 更新参数
    void UpdateParam();

    bool IdValid(unsigned short id);

    // 允许
    bool Allowed();

    // 节点有效
    bool NodeValid(unsigned short from_id, unsigned short to_id);

    bool NodeSame(unsigned short id1, unsigned short id2);

private:
    bool init_;           // 初始化完成
    bool enable_;         // 是否使能(手动模式下关闭)
    bool open_;           // 是否开启检测功能
    float max_dis_;       // 允许比理论路径距离多行走的距离(当规划当前路径还未结束)
    bool use_theory_vel_; // 使用理论速度计算/里程计计算
    
    DisRunData cur_run_data_;
    DisTargetData target_run_data_;
    std::mutex mutex_;
    TimeStamp time_stamp_;
};

